using BepuUtilities;
using System.Runtime.CompilerServices;
#if MYCODE
using BepuUtilities.Vectors;
#else
using System.Numerics;
#endif
namespace BepuPhysics.Constraints.Contact
{
    /// <summary>
    /// 处理切线摩擦实现。
    /// </summary>
    public static class TangentFriction
    {
        public struct Projection
        {
            // 在飞翔上由切线和偏移生成雅可比。
            // 切线是从曲面基础重建的。
            // 这样,每个约束相对于两个共享的线性Jacobian和四个角Jacobian的半自然基线节省了11个浮点数。
            public Vector3Wide OffsetA;
            public Vector3Wide OffsetB;
            public Symmetric2x2Wide EffectiveMass;
        }

        public struct Jacobians
        {
            public Matrix2x3Wide LinearA;
            public Matrix2x3Wide AngularA;
            public Matrix2x3Wide AngularB;
        }
        // 因为这是一个非共享的专用实现,所以雅可比计算保存在这里,而不是批处理中。
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static void ComputeJacobians(ref Vector3Wide tangentX, ref Vector3Wide tangentY, ref Vector3Wide offsetA, ref Vector3Wide offsetB,
            out Jacobians jacobians)
        {
            // 两个速度约束：
            // 点(速度(p,A),切线X)=点(速度(p,B),切线X)
            // 点(速度(p,A),切线Y)=点(速度(p,B),切线Y)
            // 其中,速度(p,A)是附着在对象A上的点p的速度。
            // 速度(p,A)=线速度A+角速度A x(p位置a)=线速度A+角速度A x偏移A
            // 因此：
            // 点(速度(p,A),切线X)=点(线性速度A,切线X)+点(角度速度A x偏移A,切线X)
            // 点(速度(p,A),切线X)=点(线性速度A,切线X)+点(偏移A x切线X,角度速度A)
            // 重申两个约束：
            // 点(线性速度A,切线X)+点(偏移A x切线X,角度速度A)=点(线性速度B,切线X)+点(偏移B x切线X,角度速度B)
            // 点(线性速度A,相切Y)+点(偏移A x相切Y,角度速度A)=点(线性速度B,相切Y)+点(偏移B x相切Y,角度速度B)
            // 点(线性速度A,切线X)+点(偏移A x切线X,角度速度A)-点(线性速度B,切线X)-点(偏移B x切线X,角度速度B)=0
            // 点(线性速度A,相切Y)+点(偏移A x相切Y,角度速度A)-点(线性速度B,相切Y)-点(偏移B x相切Y,角度速度B)=0

            // 由于有两个约束(2个自由度),因此雅可比中有两行,基于上面的是：
            // jLinearA=[切线X]
            // [切线Y]
            // jAngularA=[偏移A x切线X]
            // [偏移A x切线Y]
            // jLinearB=[-切线X]
            // [-切线Y]
            // jAngularB=[-offsetB x切线X]=[切线X x偏移量B]
            // [-OffsetB x相切Y][相切Y x偏移B]
            // TODO：手动删除此副本有一个小小的好处,因为编译器很可能不会这样做,而且它可能还会引入更多的本地init。
            jacobians.LinearA.X = tangentX;
            jacobians.LinearA.Y = tangentY;
            Vector3Wide.CrossWithoutOverlap(offsetA, tangentX, out jacobians.AngularA.X);
            Vector3Wide.CrossWithoutOverlap(offsetA, tangentY, out jacobians.AngularA.Y);
            Vector3Wide.CrossWithoutOverlap(tangentX, offsetB, out jacobians.AngularB.X);
            Vector3Wide.CrossWithoutOverlap(tangentY, offsetB, out jacobians.AngularB.Y);
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static void Prestep(ref Vector3Wide tangentX, ref Vector3Wide tangentY, ref Vector3Wide offsetA, ref Vector3Wide offsetB,
            ref BodyInertias inertiaA, ref BodyInertias inertiaB,
            out Projection projection)
        {
            ComputeJacobians(ref tangentX, ref tangentY, ref offsetA, ref offsetB, out var jacobians);
            // 计算有效质量矩阵贡献。
            Symmetric2x2Wide.SandwichScale(jacobians.LinearA, inertiaA.InverseMass, out var linearContributionA);
            Symmetric2x2Wide.SandwichScale(jacobians.LinearA, inertiaB.InverseMass, out var linearContributionB);

            Symmetric3x3Wide.MatrixSandwich(jacobians.AngularA, inertiaA.InverseInertiaTensor, out var angularContributionA);
            Symmetric3x3Wide.MatrixSandwich(jacobians.AngularB, inertiaB.InverseInertiaTensor, out var angularContributionB);

            // 没有软化;此约束在设计上是刚性的。(它确实支持最大力,但这与适当的阻尼比/固有频率不同。)
            Symmetric2x2Wide.Add(linearContributionA, linearContributionB, out var linear);
            Symmetric2x2Wide.Add(angularContributionA, angularContributionB, out var angular);
            Symmetric2x2Wide.Add(linear, angular, out var inverseEffectiveMass);
            Symmetric2x2Wide.InvertWithoutOverlap(inverseEffectiveMass, out projection.EffectiveMass);
            projection.OffsetA = offsetA;
            projection.OffsetB = offsetB;

            // 请注意,摩擦约束没有偏移速度。他们的目标是零速度。
        }

        /// <summary>
        /// 将冲量从约束空间变换到世界空间,并使用它修改身体的缓存世界空间速度。
        /// </summary>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static void ApplyImpulse(ref Jacobians jacobians, ref BodyInertias inertiaA, ref BodyInertias inertiaB,
            ref Vector2Wide correctiveImpulse, ref BodyVelocities wsvA, ref BodyVelocities wsvB)
        {
            Matrix2x3Wide.Transform(correctiveImpulse, jacobians.LinearA, out var linearImpulseA);
            Matrix2x3Wide.Transform(correctiveImpulse, jacobians.AngularA, out var angularImpulseA);
            Matrix2x3Wide.Transform(correctiveImpulse, jacobians.AngularB, out var angularImpulseB);
            BodyVelocities correctiveVelocityA, correctiveVelocityB;
            Vector3Wide.Scale(linearImpulseA, inertiaA.InverseMass, out correctiveVelocityA.Linear);
            Symmetric3x3Wide.TransformWithoutOverlap(angularImpulseA, inertiaA.InverseInertiaTensor, out correctiveVelocityA.Angular);
            Vector3Wide.Scale(linearImpulseA, inertiaB.InverseMass, out correctiveVelocityB.Linear);
            Symmetric3x3Wide.TransformWithoutOverlap(angularImpulseB, inertiaB.InverseInertiaTensor, out correctiveVelocityB.Angular);
            Vector3Wide.Add(wsvA.Linear, correctiveVelocityA.Linear, out wsvA.Linear);
            Vector3Wide.Add(wsvA.Angular, correctiveVelocityA.Angular, out wsvA.Angular);
            Vector3Wide.Subtract(wsvB.Linear, correctiveVelocityB.Linear, out wsvB.Linear); // 注意减法-我们基于线性A雅可比。
            Vector3Wide.Add(wsvB.Angular, correctiveVelocityB.Angular, out wsvB.Angular);
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static void WarmStart(ref Vector3Wide tangentX, ref Vector3Wide tangentY, ref TangentFriction.Projection projection, ref BodyInertias inertiaA, ref BodyInertias inertiaB,
            ref Vector2Wide accumulatedImpulse, ref BodyVelocities wsvA, ref BodyVelocities wsvB)
        {
            ComputeJacobians(ref tangentX, ref tangentY, ref projection.OffsetA, ref projection.OffsetB, out var jacobians);
            // TODO：如果上一帧和当前帧与不同的时间步长相关联,则上一帧的解决方案将不再是一个好的解决方案。
            // 为了补偿这一点,如果DT改变,应该对累积的脉冲进行定标。
            ApplyImpulse(ref jacobians, ref inertiaA, ref inertiaB, ref accumulatedImpulse, ref wsvA, ref wsvB);
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static void ComputeCorrectiveImpulse(ref BodyVelocities wsvA, ref BodyVelocities wsvB, ref TangentFriction.Projection data, ref Jacobians jacobians,
            ref Vector<float> maximumImpulse, ref Vector2Wide accumulatedImpulse, out Vector2Wide correctiveCSI)
        {
            Matrix2x3Wide.TransformByTransposeWithoutOverlap(wsvA.Linear, jacobians.LinearA, out var csvaLinear);
            Matrix2x3Wide.TransformByTransposeWithoutOverlap(wsvA.Angular, jacobians.AngularA, out var csvaAngular);
            Matrix2x3Wide.TransformByTransposeWithoutOverlap(wsvB.Linear, jacobians.LinearA, out var csvbLinear);
            Matrix2x3Wide.TransformByTransposeWithoutOverlap(wsvB.Angular, jacobians.AngularB, out var csvbAngular);
            // 请注意,约束空间中的速度为(csva线性-csvb线性+csvaAngular+csvbAngular)。
            // 那里的减法是由于在两个物体之间共享线性雅可比。
            // 在下面,我们需要计算约束空间的"违反"速度-它是约束空间中上述速度的否定。
            // 因此,(csvb线性-csva线性-(csvaAngular+csvbAngular))。
            Vector2Wide.Subtract(csvbLinear, csvaLinear, out var csvLinear);
            Vector2Wide.Add(csvaAngular, csvbAngular, out var csvAngular);
            Vector2Wide.Subtract(csvLinear, csvAngular, out var csv);

            Symmetric2x2Wide.TransformWithoutOverlap(csv, data.EffectiveMass, out var csi);

            var previousAccumulated = accumulatedImpulse;
            Vector2Wide.Add(accumulatedImpulse, csi, out accumulatedImpulse);
            // 最大摩擦力取决于法向冲量。每次迭代都会提供最大值。
            Vector2Wide.Length(accumulatedImpulse, out var accumulatedMagnitude);
            // 注意零位保护的除法。
            var scale = Vector.Min(Vector<float>.One, maximumImpulse / Vector.Max(new Vector<float>(1e-16f), accumulatedMagnitude));
            Vector2Wide.Scale(accumulatedImpulse, scale, out accumulatedImpulse);

            Vector2Wide.Subtract(accumulatedImpulse, previousAccumulated, out correctiveCSI);

        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static void Solve(ref Vector3Wide tangentX, ref Vector3Wide tangentY, ref TangentFriction.Projection projection, ref BodyInertias inertiaA, ref BodyInertias inertiaB, ref Vector<float> maximumImpulse, ref Vector2Wide accumulatedImpulse, ref BodyVelocities wsvA, ref BodyVelocities wsvB)
        {
            ComputeJacobians(ref tangentX, ref tangentY, ref projection.OffsetA, ref projection.OffsetB, out var jacobians);
            ComputeCorrectiveImpulse(ref wsvA, ref wsvB, ref projection, ref jacobians, ref maximumImpulse, ref accumulatedImpulse, out var correctiveCSI);
            ApplyImpulse(ref jacobians, ref inertiaA, ref inertiaB, ref correctiveCSI, ref wsvA, ref wsvB);

        }

    }
}
